#include <pthread.h>
#include <sys/mutexqueue.h>
#include <arch/xchg.h>
#include <sys/proc.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>

int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *mutexattr)
{
    if (!mutex)
        return EINVAL;
    *mutex = (pthread_mutex_t)PTHREAD_MUTEX_INITIALIZER;
    if (mutexattr)
    {
        memcpy(&mutex->attr, mutexattr, sizeof(pthread_mutexattr_t));
    }
    mutex->kind = mutex->attr.type;
    mutex->mutex_queue = mutex_queue_alloc();
    if (mutex->mutex_queue < 0)
        return ENOMEM;
    return 0;
}

int pthread_mutex_destroy(pthread_mutex_t *mutex)
{
    if (!mutex)
        return EINVAL;
    mutex->count = 0;
    mutex->owner = 0;
    mutex->kind = 0;
    //free mutex queue
    if (mutex->mutex_queue >= 0)
        mutex_queue_free(mutex->mutex_queue);
    mutex->mutex_queue = -1;
    mutex->lock = 0;
    pthread_spin_init(&mutex->spin, PTHREAD_PROCESS_PRIVATE);
    memset(&mutex->attr, 0, sizeof(pthread_mutexattr_t));
    return 0;
}

int pthread_mutex_lock(pthread_mutex_t *mutex)
{
    if (!mutex)
        return EINVAL;
    //static create
    if (mutex->mutex_queue < 0)
    {
        mutex->mutex_queue = mutex_queue_alloc();
        if (mutex->mutex_queue < 0)
            return ENOMEM;
    }

    if (mutex->kind == PTHREAD_MUTEX_NORMAL) //normal lock
    {
        while (1)
        {
            pthread_spin_lock(&mutex->spin); //get mutex lock operator permission
            if (!test_and_set(&mutex->lock, 0))
            {
                mutex_queue_wait(mutex->mutex_queue, &mutex->spin, MUTEX_QUEUE_ZERO, 0);
            }
            else
            {
                //get lock
                mutex->lock = 0;
                mutex->owner = pthread_self();
                pthread_spin_unlock(&mutex->spin);
                break;
            }
        }
    }
    else
    {
        if (mutex->kind == PTHREAD_MUTEX_ERRORCHECK) //check lock
        {
            while (1)
            {
                pthread_spin_lock(&mutex->spin); //get operator permission

                if (mutex->owner > 0) //lock had be any thread hold
                {
                    if (mutex->owner == pthread_self()) //reapt get lock
                    {
                        pthread_spin_unlock(&mutex->spin);
                        return EDEADLK;
                    }
                    else //lock is be holding by other thread
                    {
                        pthread_spin_unlock(&mutex->spin);
                        return EPERM;
                    }
                }

                //mutex lock had be hold by other pthread
                if (!test_and_set(&mutex->lock, 0))
                {
                    mutex_queue_wait(mutex->mutex_queue, &mutex->spin, MUTEX_QUEUE_ZERO, 0);
                }
                else
                {
                    mutex->lock = 0;
                    mutex->owner = pthread_self();
                    pthread_spin_unlock(&mutex->spin);
                }
            }
        }
        else
        {
            if (mutex->kind == PTHREAD_MUTEX_RECURSIVE) //recursive lock
            {
                while (1)
                {
                    pthread_spin_lock(&mutex->spin);

                    if (mutex->owner > 0) //had any thread hold lock
                    {
                        if (mutex->owner == pthread_self())
                        {
                            mutex->count++;
                            pthread_spin_unlock(&mutex->spin);
                            return 0;
                        }
                        else
                        {
                            //lock be hold by other thread
                            pthread_spin_unlock(&mutex->spin);
                            return EPERM;
                        }
                    }

                    //first get lock must be count is 0
                    if (mutex->count > 0)
                    {
                        pthread_spin_unlock(&mutex->spin);
                        return EDEADLK;
                    }

                    if (!test_and_set(&mutex->lock, 0))
                    {
                        mutex_queue_wait(mutex->mutex_queue, &mutex->spin, MUTEX_QUEUE_ZERO, 0);
                    }
                    else
                    {
                        mutex->lock = 0;
                        mutex->owner = pthread_self();
                        pthread_spin_unlock(&mutex->spin);
                        break;
                    }
                }
            }
        }
    }
    return 0;
}

int pthread_mutex_unlock(pthread_mutex_t *mutex)
{
    if (!mutex)
        return EINVAL;

    //realloc mutex queue
    if (mutex->mutex_queue < 0)
    {
        mutex->mutex_queue = mutex_queue_alloc();
        if (mutex->mutex_queue)
            return ENOMEM;
    }

    if (mutex->kind == PTHREAD_MUTEX_NORMAL) //normal lock
    {
        pthread_spin_lock(&mutex->spin);
        mutex->lock = 1;
        mutex->owner = 0;
        mutex_queue_wake(mutex->mutex_queue, NULL, 0, 0);
        pthread_spin_unlock(&mutex->spin);
    }
    else
    {
        if (mutex->kind == PTHREAD_MUTEX_ERRORCHECK) //error check lock
        {
            pthread_spin_lock(&mutex->spin);
            if (mutex->lock == 1) //try to unlock the had been unlocked lock
            {
                pthread_spin_unlock(&mutex->spin);
                return EPERM;
            }
            mutex->lock = 1;
            mutex->owner = 0;
            mutex_queue_wake(mutex->mutex_queue, NULL, 0, 0);
            pthread_spin_unlock(&mutex->spin);
        }
        else
        {
            if (mutex->kind == PTHREAD_MUTEX_RECURSIVE) //recursive lock
            {
                pthread_spin_lock(&mutex->spin);
                if (mutex->lock == 1)
                {
                    pthread_spin_unlock(&mutex->spin);
                    return EPERM;
                }

                if (mutex->owner > 0) //had be hold by andy pthread
                {
                    if (mutex->owner == pthread_self()) //holder is self
                    {
                        if (mutex->count > 0)
                        {
                            mutex->count = 0;
                            pthread_spin_unlock(&mutex->spin);
                        }
                    }
                }
                mutex->lock = 1;
                mutex->owner = 0;
                mutex_queue_wake(mutex->mutex_queue, NULL, 0, 0);
                pthread_spin_unlock(&mutex->spin);
            }
        }
    }
    return 0;
}

int pthread_mutex_trylock(pthread_mutex_t *mutex)
{
    if (!mutex)
        return EINVAL;

    if (mutex->kind == PTHREAD_MUTEX_NORMAL) //normal lock
    {
        pthread_spin_lock(&mutex->spin);
        if (!test_and_set(&mutex->lock, 0))
        {
            pthread_spin_unlock(&mutex->spin);
            return EBUSY;
        }
        mutex->lock = 0;
        mutex->owner = pthread_self();
        pthread_spin_unlock(&mutex->spin);
    }
    else
    {
        if (mutex->kind == PTHREAD_MUTEX_ERRORCHECK) //error check lock
        {
            pthread_spin_lock(&mutex->spin);
            if (mutex->owner > 0) //lock had be hold by any pthread
            {
                if (mutex->owner == pthread_self()) //repeat lock
                {
                    pthread_spin_unlock(&mutex->spin);
                    return EDEADLK;
                }
                else //lock be hold by other pthread
                {
                    pthread_spin_unlock(&mutex->spin);
                    return EPERM;
                }
            }
            //free and can get lock
            if (test_and_set(&mutex->lock, 0))
            {
                pthread_spin_unlock(&mutex->spin);
                return EBUSY;
            }
            mutex->owner = pthread_self();
            mutex->lock = 0;
            pthread_spin_unlock(&mutex->spin);
        }
        else
        {
            if (mutex->kind == PTHREAD_MUTEX_RECURSIVE) //recursive lock
            {
                pthread_spin_lock(&mutex->spin);
                if (mutex->owner > 0)
                {
                    //owner is itself
                    if (mutex->owner == pthread_self())
                    {
                        mutex->count++;
                        pthread_spin_unlock(&mutex->spin);
                        return 0;
                    }
                    else //other pthread hold lock
                    {
                        pthread_spin_unlock(&mutex->spin);
                        return EBUSY;
                    }
                }
                //no owner
                if (!test_and_set(&mutex->lock, 0))
                {
                    pthread_spin_unlock(&mutex->spin);
                    return EBUSY;
                }
                mutex->owner = pthread_self();
                mutex->lock = 0;
                pthread_spin_unlock(&mutex->spin);
            }
        }
    }
    return 0;
}

int pthread_mutexattr_init(pthread_mutexattr_t *attr)
{
    if (!attr)
        return EINVAL;
    attr->pshared = PTHREAD_PROCESS_PRIVATE;
    attr->type = PTHREAD_MUTEX_DEFAULT;
    return 0;
}

int pthread_mutexattr_destroy(pthread_mutexattr_t *attr)
{
    if (!attr)
        return EINVAL;
    attr->pshared = 0;
    attr->type = 0;
    return 0;
}

int pthread_mutexattr_setpshared(pthread_mutexattr_t *attr, int pshared)
{
    if (!attr)
        return EINVAL;
    attr->pshared = pshared;
    return 0;
}

int pthread_mutexattr_getpshared(pthread_mutexattr_t *attr, int *pshared)
{
    if (!attr)
        return EINVAL;
    if (pshared)
        *pshared = attr->pshared;
    return 0;
}

int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int type)
{
    if (!attr)
        return EINVAL;
    attr->type = type;
    return 0;
}

int pthread_mutexattr_gettype(pthread_mutexattr_t *attr, int *type)
{
    if (!attr)
        return EINVAL;
    if (type)
        *type = attr->type;
    return 0;
}
